/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
// -*- coding:iso-8859-15 mode: c++ -*-
/** 
 * \file
 * \brief MaCI_CoordinateDrive Logger implementation.
 * \author Matthieu Myrsky <matthieu.myrsky@tkk.fi>
 *
 * $Id: CoordinateDriveLogger.cpp,v 1.3 2009-06-24 16:49:25 morsko Exp $
 *
 */
#include "CoordinateDriveLogger.hpp"
#include "gimutils.h"



CoordinateDriveLogger::CoordinateDriveLogger(MaCI::CoordinateDrive::CCoordinateDriveClient *aClient,
                                             std::string aMaCISLName):
  CSync(1, 1),               
  CThread(2),                
  Logger(),
  iCDClient(aClient),
  iRunning(false),
  iFile(NULL),
  iMaCISLName(aMaCISLName),
  iAdditionalInformation("Not active"),
  iNumberOfStoredMeasurement(0),
  iNumberOfReachedPointsSaved(0)  
{

}



CoordinateDriveLogger::~CoordinateDriveLogger(){
  if(iRunning){
    StopLogging();
  }
  
}

  
bool CoordinateDriveLogger::StartLogging(void)
{
  dPrint(10,"Start called");
  if(!iRunning){
    dPrint(5,"Start logging CoordinateDrive");
    char buffer[256];
    if(iFilenameSet){
      
     
      sprintf(buffer,"data/%s.txt",
             iFilename.c_str());

    }else{
  
      gim::time tstamp(true);
      tm t;
      //      MaCI::Common::TTimestamp tstamp;
      tstamp.getLocalTime(t);
       
      sprintf(buffer,"data/%s_%.2d_%.2d_%d_%.2d:%.2d:%.2d.txt",
              iMaCISLName.c_str(),
              t.tm_mday,
              t.tm_mon+1,
              t.tm_year+1900,
              t.tm_hour,
              t.tm_min,
              t.tm_sec);
              
     
    }
    dPrint(3,"starting logging to %s",buffer);
    iFile = fopen(buffer,"wt");

    iRunning = true; 
    /////////Search the group and instance names :)
    std::string instance;
    std::string group;

    MaCI::MaCICtrl::SMaCISL sl(iMaCISLName);
    if (sl.IsValidFQMaCISL()) {
      group = sl.GetDeviceGroup();
      instance = sl.GetInstance();
    }
    //////////
   

    fprintf(iFile,"%%MaCI_CoordinateDrive log file\n");
    fprintf(iFile,"%%Group: %s Instance: %s\n",group.c_str(),instance.c_str());

    fprintf(iFile,"%%");

    fprintf(iFile,"timestamp_s timestamp_us messageType(0=coordinatesEvent; 1=LatestPointReached) numberOfCoordinates [x y a isDifferential isPathtarget isWaitPoint]");
    
    
    fprintf(iFile,"\n\n");
    RunThread(0);    
    RunThread(1);
  }

  return false;
}

 
bool CoordinateDriveLogger::StopLogging(void)
{


  if(iRunning){
    dPrint(5,"Stop logging CoordinateDrive");
    Lock();
    iRunning = false;
    
    Unlock();
    WaitThread(0);
    WaitThread(1);
    fclose(iFile);
    Lock();
    iAdditionalInformation = "Not active";
    Unlock();
  }
  return false;
}


int CoordinateDriveLogger::ThreadFunction(const int aThreadNumber)
{
  using namespace MaCI::CoordinateDrive;
  if(aThreadNumber == 0){
    int seq = 0; //Because we don't want the first coordinate which can be really old...
    MaCI::CoordinateDrive::TCoordinateArray array;
    MaCI::Common::TTimestamp tstamp;

   
  
    while(iRunning){
      char buffer[256];
      if(iCDClient->GetCoordinates(array,tstamp,&seq,2000)){
        Lock();
        
        fprintf(iFile,"%u %u 0 %u ",tstamp.timestamp_s,tstamp.timestamp_us,array.size());
         
        for(unsigned int i = 0; i< array.size();i++){
          fprintf(iFile,"%f %f %f %d %d %d ", 
                  array[i].x,
                  array[i].y,
                  array[i].a,
                  array[i].isDifferential,
                  array[i].isPathTarget,
                  array[i].isWaitPoint);
        }
        fprintf(iFile,"\n");
        fflush(iFile);
        iNumberOfStoredMeasurement++;
        Unlock();
      }

      Lock();
      sprintf(buffer,"%d Coordinate event stored and %d of reached points stored.",iNumberOfStoredMeasurement,iNumberOfReachedPointsSaved);
      iAdditionalInformation.clear();
      iAdditionalInformation.assign(buffer);
      Unlock();
    }
   
  }else if(aThreadNumber == 1){
    int seq = -1;
    MaCI::Common::TTimestamp tstamp;
    TCoordinate2D coordinate;
    while(iRunning){
      char buffer[256];
      if(iCDClient->GetLatestPointReached(coordinate,tstamp,&seq,2000)){
        Lock();
        fprintf(iFile,"%u %u 1 1 %f %f %f %d %d %d\n",
                tstamp.timestamp_s,tstamp.timestamp_us,
                coordinate.x,
                coordinate.y,
                coordinate.a,
                coordinate.isDifferential,
                coordinate.isPathTarget,
                coordinate.isWaitPoint);
        fflush(iFile);
        iNumberOfReachedPointsSaved++;
        sprintf(buffer,"%d Coordinate event stored and %d of reached points stored.",iNumberOfStoredMeasurement,iNumberOfReachedPointsSaved);
        iAdditionalInformation.clear();
        iAdditionalInformation.assign(buffer);
        Unlock();
      }
    }
   
  }
  
  return 0;
  
}

std::string CoordinateDriveLogger::GetName(){
  return "CoordinateDrive Logger";
}

bool CoordinateDriveLogger::GetAdditionalInformation(std::string &aInformation){
  Lock();
  aInformation = iAdditionalInformation;
  Unlock();
  return true;
}
